/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-10-09     huger       the first version
 */
#ifndef APPLICATIONS_MATH_MICRO_ADJUST_CAL_H_
#define APPLICATIONS_MATH_MICRO_ADJUST_CAL_H_

#include<iostream>
#include<vector>
#include<array>
#include "arm_math.h"
#include "math.h"

#define M_PI 3.14159265358979323846
using namespace std;

typedef enum
{
    NONE=0,
    UP ,
    DOWN,
    LEFT,
    RIGHT,
    IN,
    OUT,
    CACLI
}ADJUST_DIR;


class MathUtils {
public:
    //四元数转旋转矩阵
     array<array<double, 3>, 3>quaternionToRotationMatrix(double w, double x, double y, double z) {
        array<array<double, 3>, 3> R;
        R[0][0] = 1 - 2 * y * y - 2 * z * z;
        R[0][1] = 2 * x * y - 2 * z * w;
        R[0][2] = 2 * x * z + 2 * y * w;
        R[1][0] = 2 * x * y + 2 * z * w;
        R[1][1] = 1 - 2 * x * x - 2 * z * z;
        R[1][2] = 2 * y * z - 2 * x * w;
        R[2][0] = 2 * x * z - 2 * y * w;
        R[2][1] = 2 * y * z + 2 * x * w;
        R[2][2] = 1 - 2 * x * x - 2 * y * y;
        return R;
    }
    //欧拉角转旋转矩阵，输入角度为弧度
     array<array<double, 3>, 3> eulerToRotationMatrix(double roll, double pitch, double yaw) {
        array<array<double, 3>, 3> R;

        double cosRoll = cos(roll);
        double sinRoll = sin(roll);
        double cosPitch = cos(pitch);
        double sinPitch = sin(pitch);
        double cosYaw = cos(yaw);
        double sinYaw = sin(yaw);

        R[0][0] = cosYaw * cosPitch;
        R[0][1] = cosYaw * sinPitch * sinRoll - sinYaw * cosRoll;
        R[0][2] = cosYaw * sinPitch * cosRoll + sinYaw * sinRoll;
        R[1][0] = sinYaw * cosPitch;
        R[1][1] = sinYaw * sinPitch * sinRoll + cosYaw * cosRoll;
        R[1][2] = sinYaw * sinPitch * cosRoll - cosYaw * sinRoll;
        R[2][0] = -sinPitch;
        R[2][1] = cosPitch * sinRoll;
        R[2][2] = cosPitch * cosRoll;
        return R;
    }

    //求矩阵的逆矩阵
     array<array<double, 3>, 3> rotationMatrixInverse(const array<array<double, 3>, 3>& R) {
        array<array<double, 3>, 3> R_inv;

        // 转置矩阵
        R_inv[0][0] = R[0][0]; R_inv[0][1] = R[1][0]; R_inv[0][2] = R[2][0];
        R_inv[1][0] = R[0][1]; R_inv[1][1] = R[1][1]; R_inv[1][2] = R[2][1];
        R_inv[2][0] = R[0][2]; R_inv[2][1] = R[1][2]; R_inv[2][2] = R[2][2];

        return R_inv;
    }
    // 矩阵相乘函数
     array<array<double, 3>, 3> matrixMultiply(const array<array<double, 3>, 3>& A, const array<array<double, 3>, 3>& B) {
        array<array<double, 3>, 3> C = {};  // 结果矩阵，初始化为 0

        // 矩阵乘法计算 C = A * B
        for (int i = 0; i < 3; ++i) {
            for (int j = 0; j < 3; ++j) {
                for (int k = 0; k < 3; ++k) {
                    C[i][j] += A[i][k] * B[k][j];  // 按照矩阵乘法规则计算元素
                }
            }
        }

        return C;
    }

//    // 定义三轴微调平台坐标系到标定IMU的坐标系的旋转矩阵 R
//    array<array<double, 3>, 3> rotationMatrix = {
//      {0, 0, -1},
//      {1, 0, 0},
//      {0, -1, 0}
//    } ;

    array<array<double, 3>, 3> rotationMatrix_w2g;//定义三轴微调平台坐标系到固定IMU的坐标系的旋转矩阵

    array<array<double, 3>, 3> rotationMatrix_euler0;//标定处的欧拉角转旋转矩阵
    array<array<double, 3>, 3> rotationMatrix_euler1;//固定处欧拉角转旋转矩阵

    array<array<double, 3>, 3> rotationMatrix_euler0_inv;//标定处逆矩阵
    array<array<double, 3>, 3> rotationMatrix_euler;//相对矩阵,计算相对旋转矩阵（固定处姿态在标定处姿态下的姿态）


};



class MicroAdjustment {
public:
    // 构造函数，传入旋转矩阵
//    MicroAdjustment(const array<array<double, 3>, 3>& rotationMatrix)
//        : R(rotationMatrix) {}

    // 计算微调装置坐标系下的移动量
    array<double,3> calculateMovement(array<double, 3>d) {
        // 计算沿器械坐标系 z 方向移动 dz 的微调装置移动量
        double moveX = d[2] * R[0][2] + d[0] * R[0][0] + d[1] * R[0][1];
        double moveY = d[2] * R[1][2] + d[0] * R[1][0] + d[1] * R[1][1];
        double moveZ = d[2] * R[2][2] + d[0] * R[2][0] + d[2] * R[2][1];

//        // 输出结果
//        cout << "To move the instrument by (" << d[0] << ", " << d[1] << ", " << d[2] << ") in its coordinate system:" << endl;
//        cout << "Micro-adjustment system should move by:" << endl;
//        cout << "X: " << moveX << endl;
//        cout << "Y: " << moveY << endl;
//        cout << "Z: " << moveZ << endl;

        return {moveZ, moveY, moveX};
    }
    array<array<double, 3>, 3> R; // 旋转矩阵

    array<double,3>disp;


    ADJUST_DIR adjust_dir = NONE;
    int8_t cacli_status=0;
};


#endif /* APPLICATIONS_MATH_MICRO_ADJUST_CAL_H_ */
